#include "directedgraph.h"
#include "json/json.h"
#include <QDebug>
DirectedGraph::DirectedGraph(std::string path)
{
    readBesselPathsFromJson(path);
}

void DirectedGraph::addNode(const std::string& description, const std::string& id, double rotation, double x, double y, double z) {
    nodes.emplace(id, Node(description, id, rotation, x, y, z));
}

// 添加边的方法
void DirectedGraph::addEdge(const std::vector<std::pair<double, double>>& allPoints,
                            bool isStraightLine, const std::vector<LineDirect>& lineDirects) {
    // 构造 Edge 对象
    Edge edge(allPoints, isStraightLine, lineDirects);

    std::vector<std::pair<double, double>> reverseAllPoints;
    for(int i=allPoints.size()-1; i>=0; --i)
    {
        reverseAllPoints.emplace_back(allPoints[i]);
    }
    Edge edge_reverse(reverseAllPoints, isStraightLine, lineDirects);

    for(auto &lineDirect : lineDirects)
    {
        const std::string& startNode = lineDirect.start;
        const std::string& endNode = lineDirect.end;
        bool reverse = lineDirect.ItemLineReverse;

        // 确保 startNode 和 endNode 存在于图中
        if (nodes.find(startNode) != nodes.end() && nodes.find(endNode) != nodes.end()) {
            // ROS_WARN("Source node (%s) or target node (%s) ItemLineReverse %d.", startNode.c_str(), endNode.c_str(),lineDirect.ItemLineReverse);
            if(reverse)
                nodes[startNode].edges.push_back(edge_reverse);
            else
                nodes[startNode].edges.push_back(edge);
        } else {
            //ROS_ERROR("Source node (%s) or target node (%s) not found.", startNode.c_str(), endNode.c_str());
        }
    }
}

// 计算从 start_id 到 goal_id 的最短路径
std::vector<std::string> DirectedGraph::shortestPath(const std::string& start_id, const std::string& goal_id) {
    // 最小优先队列，存储 pair (距离, 节点ID)
    qDebug()<<"DirectedGraph::shortestPath="<<QString::fromStdString(start_id)<<QString::fromStdString(goal_id);
    std::priority_queue<std::pair<double, std::string>, std::vector<std::pair<double, std::string>>, std::greater<>> pq;
    std::unordered_map<std::string, double> dist;
    std::unordered_map<std::string, std::string> prev;
    std::set<std::string> visited;

    // 初始化距离
    for (const auto& [node_id, node] : nodes) {
        dist[node_id] = std::numeric_limits<double>::infinity();
    }
    dist[start_id] = 0.0;
    pq.emplace(0, start_id);

    while (!pq.empty()) {
        auto [current_distance, current_id] = pq.top();
        pq.pop();

        qDebug()<<"while --------------------"<<QString::fromStdString(current_id);
        if (visited.find(current_id) != visited.end()) {
            qDebug()<<"continue -------";
            continue;
        }
        visited.insert(current_id);

        // 如果达到了目标节点，构建路径
        if (current_id == goal_id) {
            std::vector<std::string> path;
            for (auto it = prev.begin(); it != prev.end(); ++it) {
                qDebug() << QString::fromStdString(it->first) << " <= " << QString::fromStdString(it->second);
            }
            for (std::string at = goal_id; !at.empty(); at = prev[at]) {
                path.push_back(at);
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        // 遍历当前节点的所有边
        for (const auto& edge : nodes[current_id].edges) {
            for (const auto& lineDirect : edge.lineDirectList) {
                //qDebug()<<"------------"<<QString::fromStdString(lineDirect.start)<<QString::fromStdString(lineDirect.end);
                // 确保方向正确
                if (lineDirect.start != current_id) {
                    //qDebug()<<"lineDirect.start != current_id"<<QString::fromStdString(lineDirect.start)<<QString::fromStdString(current_id);
                    continue;
                }

                std::string neighbor_id = lineDirect.end;


                // 计算欧几里得距离
                double neighbor_x = nodes[neighbor_id].ItemX;
                double neighbor_y = nodes[neighbor_id].ItemY;
                double current_x = nodes[current_id].ItemX;
                double current_y = nodes[current_id].ItemY;
                double distance = fabs(std::sqrt(std::pow(neighbor_x - current_x, 2) + std::pow(neighbor_y - current_y, 2)));

                // 如果找到更短的路径，更新
                if (current_distance + distance < dist[neighbor_id]) {
                    dist[neighbor_id] = current_distance + distance;
                    prev[neighbor_id] = current_id;
                    qDebug()<<"start="<<QString::fromStdString(lineDirect.start)<<",end="<<QString::fromStdString(neighbor_id)<<QString::number(dist[neighbor_id])<<QString::fromStdString(prev[neighbor_id]);
                    pq.emplace(dist[neighbor_id], neighbor_id);
                }
            }
        }
    }
    // 如果没有找到路径，返回空路径
    return {};
}

void DirectedGraph::readBesselPathsFromJson(const std::string &filename)
{
    std::ifstream file(filename, std::ifstream::binary);
    if (!file.is_open()) {
        throw std::runtime_error("Could not open JSON file: " + filename);
    }

    Json::Value root;
    file >> root;

    // 读取节点信息
    for(auto& node : root[TMPOINT])
    {
        std::string description = node[ITEMDESCRIPTION].asString();
        std::string item_id = node[ITEMID].asString();
        double rotation = node[ITEMROTATION].asDouble();
        double x = node[ITEMX].asDouble();
        double y = node[ITEMY].asDouble();
        double z = node[ITEMZ].asDouble();

        addNode(description, item_id, rotation, x, y, z);
    }

    Json::StreamWriterBuilder writer;
    std::string graph_json = Json::writeString(writer, root[TMPOINT]);
    // 设置到 ROS 参数服务器
    //ros::param::set("/graph_data", graph_json);

    // 读取边信息
    for(auto &edge : root[TMBEZIERLINELIST])
    {
        bool straightLine = edge[ITEMLINESTRAIGHT].asBool();

        std::vector<LineDirect> lineDirects;
        for(auto &line_direct : edge[LINEDIRECT])
        {
            std::string start = line_direct[START].asString();
            std::string end = line_direct[END].asString();
            std::string type = line_direct["Type"].asString();

            bool reverse = false;
            if(type == "start->end")
            {
                reverse = false;
            }
            else if(type == "end->start")
            {
                reverse = true;
            }

            lineDirects.emplace_back(start, end, reverse);
        }

        std::vector<std::pair<double, double>> allPoints;
        for(auto &point : edge[ALLPOINT])
        {
            double x = point[0].asDouble();
            double y = point[1].asDouble();
            allPoints.emplace_back(x, y);
        }

        addEdge(allPoints, straightLine, lineDirects);
    }

    //std::cout << "===Read path from json." << graph_json.c_str() << std::endl;
}



const Node& DirectedGraph::getNode(const std::string& node_id_str) const {
    auto it = nodes.find(node_id_str);
    if (it == nodes.end()) {
        throw std::out_of_range("Node ID not found in the graph");
    }
    return it->second;
}

Edge DirectedGraph::getEdge(const std::string& source_id, const std::string& target_id) const {
    const auto& node = nodes.at(source_id);
    for (const auto& edge : node.edges) {
        // 使用 lineDirects 进行目标节点的检查
        for (const auto& lineDirect : edge.lineDirectList) {
            if (lineDirect.end == target_id) {
                return edge;
            }
        }
    }
    throw std::out_of_range("Edge not found");
}
